#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>

#include "interface/control_interface.h"

/**
 * @brief 控制驱动类，负责接收控制指令并驱动机器人
 *
 * 该类通过订阅/cmd_vel话题获取控制指令，并将指令转换为线速度和角速度发送给控制器
 */
class ControlDriver : public rclcpp::Node {
public:
    /**
     * @brief 构造函数，初始化节点并创建控制指令的订阅者和控制器实例
     */
    ControlDriver() : Node("control_driver") {

        auto controlOpt           = rclcpp::SubscriptionOptions();
        callback_group_control_   = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        controlOpt.callback_group = callback_group_control_;
        subscription_             = create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10, std::bind(&ControlDriver::ControlCmdHandler, this, std::placeholders::_1));

        p_pub_odometry_ = create_publisher<nav_msgs::msg::Odometry>("chassis/odometry", 5);

        // 创建一个定时器，周期为20ms，发送底盘的数据
        timer_ =
            this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&ControlDriver::PublishOdometry, this));

        // 创建控制器实例
        controller_ = ControllerInterface::create();
    }

    ~ControlDriver() { RCLCPP_INFO(this->get_logger(), "Control Driver Node Stopped"); }

private:
    /**
     * @brief 控制指令处理函数
     *
     * 该函数从消息中提取线速度和角速度，并调用控制器的sendVelocity方法发送给控制器
     *
     * @param msg 控制指令消息，包含线速度和角速度信息
     */
    void ControlCmdHandler(const geometry_msgs::msg::Twist::SharedPtr msg) {
        float linear_velocity[3]  = {(float) msg->linear.x, (float) msg->linear.y, (float) msg->linear.z};
        float angular_velocity[3] = {(float) msg->angular.x, (float) msg->angular.y, (float) msg->angular.z};

        controller_->sendVelocity(linear_velocity, angular_velocity);
    }

    void PublishOdometry() {
        uint64_t           time     = controller_->getOdometry().first;
        std::vector<float> odometry = controller_->getOdometry().second;

        nav_msgs::msg::Odometry odometry_msg = nav_msgs::msg::Odometry();
        odometry_msg.header.stamp            = rclcpp::Time(time);
        odometry_msg.header.frame_id         = "odom";
        odometry_msg.child_frame_id          = "odom_chassis";
        odometry_msg.pose.pose.position.x    = odometry[0];
        odometry_msg.pose.pose.position.y    = odometry[1];
        odometry_msg.pose.pose.position.z    = odometry[2];

        odometry_msg.pose.pose.orientation.x = 0.0;
        odometry_msg.pose.pose.orientation.y = 0.0;
        odometry_msg.pose.pose.orientation.z = sin(odometry[2] / 2);
        odometry_msg.pose.pose.orientation.w = cos(odometry[2] / 2);

        p_pub_odometry_->publish(odometry_msg);
    }

    // 控制指令的订阅者
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
    // 回调组，用于确保线程安全
    rclcpp::CallbackGroup::SharedPtr callback_group_control_;
    // 发布底盘里程计
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr p_pub_odometry_;
    // 定时器
    rclcpp::TimerBase::SharedPtr timer_;
    // 控制器接口实例
    std::unique_ptr<ControllerInterface> controller_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    rclcpp::NodeOptions options;
    options.use_intra_process_comms(true);
    rclcpp::executors::SingleThreadedExecutor exec;

    auto control_driver = std::make_shared<ControlDriver>();
    exec.add_node(control_driver);

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Control Driver Started.\033[0m");

    exec.spin();

    rclcpp::shutdown();
    return 0;
}